package com.symaster.common.test.pid;

public class PIDController {
    public double kp; // Proportional control gain.
    public double ki; // Integral control gain.
    public double kd; // Derivative control gain.
    private double previousError; // Error from the previous update.
    private double integral; // Accumulation of errors over time.

    // Constructor
    public PIDController(double kp, double ki, double kd) {
        this.kp = kp;
        this.ki = ki;
        this.kd = kd;
        this.previousError = 0;
        this.integral = 0;
    }

    // Update PID calculations
    public double update(double setpoint, double actualValue, double dt) {
        double error = setpoint - actualValue;
        integral += error * dt;
        double derivative = (error - previousError) / dt;
        double output = kp * error + ki * integral + kd * derivative;
        previousError = error;
        return output;
    }

    // Resetting the controller (e.g., at the start of a new maneuver)
    public void reset() {
        previousError = 0;
        integral = 0;
    }
}